import sympy
import sympybotics
rbtdef = sympybotics.RobotDef('home', # robot name
                          [('-pi/2', 0, 0, 'q'),
                           ('pi/2', 0, 0.1415, 'q'),
                           ('-pi/2', 0, 0, 'q'),
                           ('pi/2', 0, 0.281, 'q'),
                           ('-pi/2', 0, 0, 'q'),
                           ('pi/2', 0, 0.2125, 'q'),
                           ('-pi/2', 0, 0, 'q'),
                           ], # (alpha, a, d, theta)
                            dh_convention='standard' # either 'standard' or 'modified'
                              )
rbtdef.frictionmodel = {'Coulomb', 'viscous'} # options are None or a combination of 'Coulomb', 'viscous' and 'offset'
rbtdef.dynparms()
rbt = sympybotics.RobotDynCode(rbtdef, verbose=True)
tau_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.invdyn_code, 'tau_out', 'tau', rbtdef)
print(tau_str)
f = open('home_tau_out.cpp','w')
f.write(tau_str)
f.close()
rbt.calc_base_parms()
print(rbt.dyn.baseparms)
regressor_str = sympybotics.robotcodegen.robot_code_to_func('C', rbt.H_code, 'regressor_out', 'regressor', rbtdef)
print(regressor_str)
f = open('home_regressor.cpp','w')
f.write(regressor_str)
f.close()
